Observations in the car coordinate system can be transformed into map coordinates (\text{x}_m and \text{y}_m) by passing car observation coordinates (\text{x}_c and \text{y}_c), map particle coordinates (\text{x}_p and \text{y}_p), and our rotation angle (-90 degrees) through a homogenous transformation matrix. This homogenous transformation matrix, shown below, performs rotation and translation.

Homogenous Transformation

\left[ \begin{array}{c} \text{x}_m \\ \text{y}_m \\ 1 \end{array} \right] = \begin{bmatrix} \cos\theta & -\sin\theta & \text{x}_p \\ \sin\theta & \cos\theta & \text{y}_p \\ 0 & 0 & 1 \end{bmatrix} \times \left[ \begin{array}{c} \text{x}_c \\ \text{y}_c \\ 1 \end{array} \right]

Matrix multiplication results in:

\text{x}_m= \text{x}_p + (\cos\theta \times \text{x}_c) - (\sin\theta \times \text{y}_c)

\text{y}_m= \text{y}_p + (\sin\theta \times \text{x}_c) + (\cos\theta \times \text{y}_c)

Quiz Solutions

The solutions are implemented in python, for clarity.

Observation 1 Solution

import numpy as np

# define coordinates and theta
x_part= 4
y_part= 5
x_obs= 2
y_obs= 2
theta= -np.pi/2 # -90 degrees

# transform to map x coordinate
x_map= x_part + (np.cos(theta) * x_obs) - (np.sin(theta) * y_obs)

# transform to map y coordinate
y_map= y_part + (np.sin(theta) * x_obs) + (np.cos(theta) * y_obs)

print(int(x_map), int(y_map)) # (6,3)

Observation 2 Solution

import numpy as np

# define coordinates and theta
x_part= 4
y_part= 5
x_obs= 3
y_obs= -2
theta= -np.pi/2 # -90 degrees

# transform to map x coordinate
x_map= x_part + (np.cos(theta) * x_obs) - (np.sin(theta) * y_obs)

# transform to map y coordinate
y_map= y_part + (np.sin(theta) * x_obs) + (np.cos(theta) * y_obs)

print(int(x_map), int(y_map)) # (2,2)

Observation 3 Solution

import numpy as np

# define coordinates and theta
x_part= 4
y_part= 5
x_obs= 0
y_obs= -4
theta= -np.pi/2 # -90 degrees

# transform to map x coordinate
x_map= x_part + (np.cos(theta) * x_obs) - (np.sin(theta) * y_obs)

# transform to map y coordinate
y_map= y_part + (np.sin(theta) * x_obs) + (np.cos(theta) * y_obs)

print(int(x_map), int(y_map)) # (0,5)